/*******************************************************************************************/
/* The MIT License (MIT)                                                                   */
/*                                                                                         */
/* Copyright (c) 2014 - Marina High School FIRST Robotics Team 4276 (Huntington Beach, CA) */
/*                                                                                         */
/* Permission is hereby granted, free of charge, to any person obtaining a copy            */
/* of this software and associated documentation files (the "Software"), to deal           */
/* in the Software without restriction, including without limitation the rights            */
/* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell               */
/* copies of the Software, and to permit persons to whom the Software is                   */
/* furnished to do so, subject to the following conditions:                                */
/*                                                                                         */
/* The above copyright notice and this permission notice shall be included in              */
/* all copies or substantial portions of the Software.                                     */
/*                                                                                         */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR              */
/* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,                */
/* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE             */
/* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER                  */
/* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,           */
/* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN               */
/* THE SOFTWARE.                                                                           */
/*******************************************************************************************/

/*******************************************************************************************/
/* We are a high school robotics team and always in need of financial support.             */
/* If you use this software for commercial purposes please return the favor and donate     */
/* (tax free) to "Marina High School Educational Foundation"  (Huntington Beach, CA)       */
/*******************************************************************************************/

#include <stdio.h>
#include <string>
#include <signal.h>
#include <unistd.h>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#include "CBallCircle.h"
#include "CTargetInfo.h"
#include "CVideoFrame.h"
#include "CVideoFrameQueue.h"
#include "CConnectionServer.h"
#include "CGpioLed.h"
#include "CBallDetector.h"
#include "CTestMonitor.h"
#include "CFrameGrinder.h"
#include "dbgMsg.h"
#include "viking_cv_version.h"

CBallDetector::CBallDetector()
{
}

CBallDetector::CBallDetector(const CBallDetector& orig)
{
}

CBallDetector::~CBallDetector()
{
}

void CBallDetector::init()
{
    m_tolerancePercentForRadius = 0.20;
    initLookupTable();
}

void CBallDetector::initLookupTable()
{
    INIT_RADIUS_TABLE_ITEM initTable[][MEASUREMENT_ARRAY_Y_WIDTH] = {
        {
            {MEAS_X_NEAR_EDGE, MEAS_Y_LEFT_EDGE, 45.0, 4.5, -35.4}, //    [0, 0]    (3)
            {MEAS_X_NEAR_EDGE, MEAS_Y_LEFT, 45.0, 4.5, -35.4}, //         [0, 1]    (3)
            {MEAS_X_NEAR_EDGE, MEAS_Y_MID_LEFT, 55.0, 3.5, -17.5}, //     [0, 2]    (3)
            {MEAS_X_NEAR_EDGE, MEAS_Y_CENTER, 50.0, 2.5, 0.0}, //         [0, 3]    (3)
            {MEAS_X_NEAR_EDGE, MEAS_Y_MID_RIGHT, 55.0, 3.5, 17.5}, //     [0, 4]    (3)
            {MEAS_X_NEAR_EDGE, MEAS_Y_RIGHT, 45.0, 4.5, 35.4}, //         [0, 5]    (3)
            {MEAS_X_NEAR_EDGE, MEAS_Y_RIGHT_EDGE, 45.0, 4.5, 35.4} //     [0, 6]    (3)
        },
        {
            {MEAS_X_NEAR, MEAS_Y_LEFT_EDGE, 45.0, 4.5, -35.4}, //         [1, 0]    (3)
            {MEAS_X_NEAR, MEAS_Y_LEFT, 45.0, 4.5, -35.4}, //              [1, 1]    (1)
            {MEAS_X_NEAR, MEAS_Y_MID_LEFT, 55.0, 3.5, -17.5}, //          [1, 2] // (2) This value is interpolated from other data.  Too hard to measure so close to camera - target is huge and center hard to find)
            {MEAS_X_NEAR, MEAS_Y_CENTER, 50.0, 2.5, 0.0}, //              [1, 3]
            {MEAS_X_NEAR, MEAS_Y_MID_RIGHT, 55.0, 3.5, 17.5}, //          [1, 4]
            {MEAS_X_NEAR, MEAS_Y_RIGHT, 45.0, 4.5, 35.4}, //              [1, 5]    (1)
            {MEAS_X_NEAR, MEAS_Y_RIGHT_EDGE, 45.0, 4.5, 35.4} //          [1, 6]    (3)
        },
        {
            {MEAS_X_MID_NEAR, MEAS_Y_LEFT_EDGE, 34.0, 7.5, -26.6}, //     [2, 0]    (3)
            {MEAS_X_MID_NEAR, MEAS_Y_LEFT, 34.0, 7.5, -26.6}, //          [2, 1]    (1)
            {MEAS_X_MID_NEAR, MEAS_Y_MID_LEFT, 36.5, 6.4, -15.6}, //      [2, 2]
            {MEAS_X_MID_NEAR, MEAS_Y_CENTER, 35.3, 6.7, 0.0}, //          [2, 3]
            {MEAS_X_MID_NEAR, MEAS_Y_MID_RIGHT, 36.5, 6.4, 15.6}, //      [2, 4]
            {MEAS_X_MID_NEAR, MEAS_Y_RIGHT, 34.0, 7.5, 26.6}, //          [2, 5]    (1)
            {MEAS_X_MID_NEAR, MEAS_Y_RIGHT_EDGE, 34.0, 7.5, 26.6} //      [2, 6]    (3)
        },
        {
            {MEAS_X_CENTER, MEAS_Y_LEFT_EDGE, 24.0, 10.9, -24.6}, //      [3, 0]    (3)
            {MEAS_X_CENTER, MEAS_Y_LEFT, 24.0, 10.9, -24.6}, //           [3, 1] // (1) Corrected pixel radius to allow for partial outside FOV in measurement
            {MEAS_X_CENTER, MEAS_Y_MID_LEFT, 28.2, 10.6, -13.7}, //       [3, 2]
            {MEAS_X_CENTER, MEAS_Y_CENTER, 26.0, 10.9, 0.0}, //           [3, 3]
            {MEAS_X_CENTER, MEAS_Y_MID_RIGHT, 28.2, 10.6, 13.7}, //       [3, 4]
            {MEAS_X_CENTER, MEAS_Y_RIGHT, 24.0, 10.9, 24.6}, //           [3, 5]    (1)
            {MEAS_X_CENTER, MEAS_Y_RIGHT_EDGE, 24.0, 10.9, 24.6} //       [3, 6]    (3)
        },
        {
            {MEAS_X_MID_FAR, MEAS_Y_LEFT_EDGE, 19.1, 13.6, -22.9}, //     [4, 0]    (3)
            {MEAS_X_MID_FAR, MEAS_Y_LEFT, 19.1, 13.6, -22.9}, //          [4, 1]
            {MEAS_X_MID_FAR, MEAS_Y_MID_LEFT, 21.5, 14.7, -11.5}, //      [4, 2]
            {MEAS_X_MID_FAR, MEAS_Y_CENTER, 20.0, 14.7, 0.0}, //          [4, 3]
            {MEAS_X_MID_FAR, MEAS_Y_MID_RIGHT, 21.5, 14.7, 11.5}, //      [4, 4]
            {MEAS_X_MID_FAR, MEAS_Y_RIGHT, 19.1, 13.6, 22.9}, //          [4, 5]
            {MEAS_X_MID_FAR, MEAS_Y_RIGHT_EDGE, 19.1, 13.6, 22.9} //      [4, 6]    (3)
        },
        {
            {MEAS_X_FAR, MEAS_Y_LEFT_EDGE, 14.8, 19.8, -31.0}, //         [5, 0]    (3)
            {MEAS_X_FAR, MEAS_Y_LEFT, 14.8, 19.8, -31.0}, //              [5, 1]
            {MEAS_X_FAR, MEAS_Y_MID_LEFT, 14.0, 19.5, -11.6}, //          [5, 2]
            {MEAS_X_FAR, MEAS_Y_CENTER, 12.4, 23.0, 0.0}, //              [5, 3]
            {MEAS_X_FAR, MEAS_Y_MID_RIGHT, 14.0, 19.5, 11.6}, //          [5, 4]
            {MEAS_X_FAR, MEAS_Y_RIGHT, 14.8, 19.8, 31.0}, //              [5, 5]
            {MEAS_X_FAR, MEAS_Y_RIGHT_EDGE, 14.8, 19.8, 31.0} //          [5, 6]    (3)
        },
        {
            {MEAS_X_FAR_EDGE, MEAS_Y_LEFT_EDGE, 14.8, 19.8, -31.0}, //    [6, 0] // (3) These sections were added to fill the table out to the edges with same data as the point where the ball sarted leaving the FOV
            {MEAS_X_FAR_EDGE, MEAS_Y_LEFT, 14.8, 19.8, -31.0}, //         [6, 1]    (3)
            {MEAS_X_FAR_EDGE, MEAS_Y_MID_LEFT, 14.0, 19.5, -11.6}, //     [6, 2]    (3)
            {MEAS_X_FAR_EDGE, MEAS_Y_CENTER, 12.4, 23.0, 0.0}, //         [6, 3]    (3)
            {MEAS_X_FAR_EDGE, MEAS_Y_MID_RIGHT, 14.0, 19.5, 11.6}, //     [6, 4]    (3)
            {MEAS_X_FAR_EDGE, MEAS_Y_RIGHT, 14.8, 19.8, 31.0}, //         [6, 5]    (3)
            {MEAS_X_FAR_EDGE, MEAS_Y_RIGHT_EDGE, 14.8, 19.8, 31.0} //     [6, 6]    (3)
        },
    };

    unsigned int i = 0;
    unsigned int j = 0;
    for (i = 0; i < VIEW_PIXEL_X_HEIGHT - 1; i++)
    {
        for (j = 0; j < VIEW_PIXEL_Y_WIDTH - 1; j++)
        {
            m_lookupTable[i][j].angleToBallDegrees = 0.0;
            m_lookupTable[i][j].distanceToBallFeet = 0.0;
            m_lookupTable[i][j].pixelRadius = 0.0;
        }
    }
    for (i = 1; i < MEASUREMENT_ARRAY_X_HEIGHT; i++)
    {
        for (j = 1; j < MEASUREMENT_ARRAY_Y_WIDTH; j++)
        {
            fillTableSectionByInterpolatingFromCorners(
                    initTable[i][j - 1], initTable[i][j], // upper left upper right
                    initTable[i - 1][j - 1], initTable[i - 1][j]); // lower left, lower right
        }
    }
}

void CBallDetector::fillTableSectionByInterpolatingFromCorners(
        const INIT_RADIUS_TABLE_ITEM& upperLeft,
        const INIT_RADIUS_TABLE_ITEM& upperRight,
        const INIT_RADIUS_TABLE_ITEM& lowerLeft,
        const INIT_RADIUS_TABLE_ITEM& lowerRight)
{
    unsigned int i = 0;
    unsigned int j = 0;
    unsigned int iSize = 0;
    unsigned int jSize = 0;
    std::string sMsg;

    // Calculate the values along the upper and lower rows
    if (upperRight.pixelRightLeft_Y == upperLeft.pixelRightLeft_Y)
    {
        dbgMsg_s("fillTableSectionByInterpolatingFromCorners:  upperRight.pixelX == upperLeft.pixelX\n");
        return;
    }
    iSize = upperRight.pixelRightLeft_Y - upperLeft.pixelRightLeft_Y;
    if (upperRight.pixelRightLeft_Y == MEAS_Y_RIGHT_EDGE)
    {
        iSize++; // Last column is a special case - the following code would get this as the 'first' column of the section if there was another section to the right of this one.
    }
    double incrementAngleX = (upperRight.angleToBallDegrees - upperLeft.angleToBallDegrees) / iSize;
    double incrementDistanceX = (upperRight.distanceToBallFeet - upperLeft.distanceToBallFeet) / iSize;
    double incrementRadiusX = (upperRight.pixelRadius - upperLeft.pixelRadius) / iSize;
    for (i = 0; i < iSize; i++)
    {
        initLookupTableElement(
                upperLeft.pixelNearFar_X, upperLeft.pixelRightLeft_Y + i,
                upperLeft.angleToBallDegrees + (i * incrementAngleX),
                upperLeft.distanceToBallFeet + (i * incrementDistanceX),
                upperLeft.pixelRadius + (i * incrementRadiusX));
    }
    incrementAngleX = (lowerLeft.angleToBallDegrees - lowerLeft.angleToBallDegrees) / iSize;
    incrementDistanceX = (lowerLeft.distanceToBallFeet - lowerLeft.distanceToBallFeet) / iSize;
    incrementRadiusX = (lowerLeft.pixelRadius - lowerLeft.pixelRadius) / iSize;
    for (i = 0; i < iSize; i++)
    {
        initLookupTableElement(
                lowerLeft.pixelNearFar_X, lowerLeft.pixelRightLeft_Y + i,
                lowerLeft.angleToBallDegrees + (i * incrementAngleX),
                lowerLeft.distanceToBallFeet + (i * incrementDistanceX),
                lowerLeft.pixelRadius + (i * incrementRadiusX));


        // Now process each column, interpolating between the values at the top and bottom rows
        if (upperLeft.pixelNearFar_X == lowerLeft.pixelNearFar_X)
        {
            dbgMsg_s("fillTableSectionByInterpolatingFromCorners:  upperLeft.pixelY == lowerLeft.pixelY\n");
            return;
        }
        jSize = upperLeft.pixelNearFar_X - lowerLeft.pixelNearFar_X;
        double incrementAngleY = (m_lookupTable[upperLeft.pixelNearFar_X][upperLeft.pixelRightLeft_Y + i].angleToBallDegrees - m_lookupTable[lowerLeft.pixelNearFar_X][lowerLeft.pixelRightLeft_Y + i].angleToBallDegrees) / jSize;
        double incrementDistanceY = (m_lookupTable[upperLeft.pixelNearFar_X][upperLeft.pixelRightLeft_Y + i].distanceToBallFeet - m_lookupTable[lowerLeft.pixelNearFar_X][lowerLeft.pixelRightLeft_Y + i].distanceToBallFeet) / jSize;
        double incrementRadiusY = (m_lookupTable[upperLeft.pixelNearFar_X][upperLeft.pixelRightLeft_Y + i].pixelRadius - m_lookupTable[lowerLeft.pixelNearFar_X][lowerLeft.pixelRightLeft_Y + i].pixelRadius) / jSize;
        for (j = 0; j < jSize; j++)
        {
            initLookupTableElement(
                    lowerLeft.pixelNearFar_X + j, lowerLeft.pixelRightLeft_Y + i,
                    lowerLeft.angleToBallDegrees + (j * incrementAngleY),
                    lowerLeft.distanceToBallFeet + (j * incrementDistanceY),
                    lowerLeft.pixelRadius + (j * incrementRadiusY));
        }
    }
}

void CBallDetector::initLookupTableElement(unsigned int pixelNearFar_X, unsigned int pixelRightLeft_Y, double angleToBallDegrees, double distanceToBallFeet, double pixelRadius)
{
    if (pixelNearFar_X >= VIEW_PIXEL_X_HEIGHT)
    {
        dbgMsg_d1("pixelNearFar_X %d is out of bounds\n", pixelNearFar_X);
        return;
    }
    if (pixelRightLeft_Y >= VIEW_PIXEL_Y_WIDTH)
    {
        dbgMsg_d1("pixelRightLeft_Y %d is out of bounds\n", pixelRightLeft_Y);
        return;
    }
    if ((angleToBallDegrees < -180.0) || (angleToBallDegrees > 180.0))
    {
        dbgMsg_f1("angleToBallDegrees %f is out of bounds\n", angleToBallDegrees);
    }
    if ((distanceToBallFeet <= 0.0) || (distanceToBallFeet > 60.0))
    {
        dbgMsg_f1("distanceToBallFeet %f is out of bounds\n", distanceToBallFeet);
    }
    if ((pixelRadius <= 0.0) || (pixelRadius > 200))
    {
        dbgMsg_f1("pixelRadius %f is out of bounds\n", pixelRadius);
    }

    m_lookupTable[pixelNearFar_X][pixelRightLeft_Y].angleToBallDegrees = (float) angleToBallDegrees;
    m_lookupTable[pixelNearFar_X][pixelRightLeft_Y].distanceToBallFeet = (float) distanceToBallFeet;
    m_lookupTable[pixelNearFar_X][pixelRightLeft_Y].pixelRadius = (float) pixelRadius;
}

void CBallDetector::detectBlobs(CVideoFrame * pFrame, CFrameGrinder* pFrameGrinder)
{
    try
    {
        static struct timespec timeLastCameraFrame = {0};
        static struct timespec timeNow = {0};
        cv::Mat img_hsv, red_blob, blue_blob, green_blob, dstA, dstB;
        static int iCount = 0;

        int timeSinceLastCameraFrameMilliseconds = (int) CTestMonitor::getDeltaTimeMilliseconds(
                timeLastCameraFrame,
                pFrame->m_timeAddedToQueue[(int) CVideoFrame::FRAME_QUEUE_WAIT_FOR_BLOB_DETECT]);
        timeLastCameraFrame = pFrame->m_timeAddedToQueue[(int) CVideoFrame::FRAME_QUEUE_WAIT_FOR_BLOB_DETECT];

        // RBG is flawed as a way to filter based on color because the brightness is combined 
        // with the color info. 
        // Not so with HSV, where Hue and Saturation are maintained separately
        // OpenCV has a handy conversion from RGB to HSV
        cv::cvtColor(pFrame->m_frame, img_hsv, CV_BGR2HSV);

        // Filter out all but Blue hue
        cv::inRange(img_hsv, cv::Scalar(100, 75, 75), cv::Scalar(105, 255, 255), blue_blob);

        // Filter out all but red
        cv::inRange(img_hsv, cv::Scalar(0, 75, 75), cv::Scalar(5, 255, 255), dstA);
        cv::inRange(img_hsv, cv::Scalar(175, 75, 75), cv::Scalar(180, 255, 255), dstB);
        cv::bitwise_or(dstA, dstB, red_blob);

        // Filter out all but Green hue
        //cv::inRange(img_hsv, cv::Scalar(74, 50, 40), cv::Scalar(94, 200, 245), green_blob);
        cv::inRange(img_hsv, cv::Scalar(74, 50, 40), cv::Scalar(94, 200, 245), green_blob);

        // make a kernel of 4x4, all 1's
        cv::Mat kernel = cv::Mat::ones(cv::Size(2, 2), CV_8U);
        cv::erode(green_blob, green_blob, kernel);
        cv::erode(green_blob, green_blob, kernel);
        cv::erode(green_blob, green_blob, kernel);
        cv::dilate(green_blob, green_blob, kernel);
        cv::dilate(green_blob, green_blob, kernel);
        cv::dilate(green_blob, green_blob, kernel);
        ///cv::GaussianBlur(green_blob,green_blob,cv::Size(5,5),1.5);

        //cv::imshow("green_blob", green_blob);
        //cv::waitKey(0);
        iCount++;
        if ((iCount % 17) == 0)
        {
            pFrameGrinder->m_testMonitor.saveFrameToJpeg(green_blob);
        }


        //Find the contours. Use the contourOutput Mat so the original image doesn't get overwritten
        std::vector<std::vector<cv::Point> > blueContours, redContours, greenContours;
        cv::findContours(blue_blob, blueContours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
        cv::findContours(red_blob, redContours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
        cv::findContours(green_blob, greenContours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);

        CBallCircle bestBallCircleBlue;
        float angleToBlueBallDegrees = 0.0;
        float distanceToBlueBallFeet = 0.0;
        bool isBlueFound = false;
#ifdef DETECT_LARGEST_BLOB_NO_FILTER_BASED_ON_SIZE
        isBlueFound = filterContoursToFindLargestBlob(blueContours, bestBallCircleBlue, angleToBlueBallDegrees, distanceToBlueBallFeet);

        isBlueFound = true;
#ifdef DISPLAY_CALIBRATION_INFO
        printf("viking_cv version %d.%d.%d", VERSION_YEAR, VERSION_INTERFACE, VERSION_BUILD);
        if (isBlueFound)
        {
            printf("   NearFar_X: %d    LeftRight_Y: %d    Radius  %02f\r",
                    (int) bestBallCircleBlue.m_ptCenter.x,
                    (int) bestBallCircleBlue.m_ptCenter.y,
                    bestBallCircleBlue.m_radius);
        }
        else
        {
            printf("   Blue ball *NOT* found\r");
        }
#endif

#else
        isBlueFound = filterContoursToFindBallBySize(blueContours, bestBallCircleBlue, angleToBlueBallDegrees, distanceToBlueBallFeet);
#endif

        CBallCircle bestBallCircleRed;
        float angleToRedBallDegrees = 0.0;
        float distanceToRedBallFeet = 0.0;
        bool isRedFound = false;
#ifdef DETECT_LARGEST_BLOB_NO_FILTER_BASED_ON_SIZE
        isRedFound = filterContoursToFindLargestBlob(redContours, bestBallCircleRed, angleToRedBallDegrees, distanceToRedBallFeet);

#else    
        isRedFound = filterContoursToFindBallBySize(redContours, bestBallCircleRed, angleToRedBallDegrees, distanceToRedBallFeet);
#endif

        bool isRightGreenTargetLit = false;
        bool isLeftGreenTargetLit = false;
        double idealAspectRatioStatic = 32.0 / 4.0; // Vertical target is 2' 8" x 4"
        double idealAspectRatioDynamic = 4.0 / 23.5; // Horizontal target is 4" x 1' 11.5"
        double aspectRatio, diff, area, predict;
        cv::RotatedRect tempRect, vertRect, horizRect;
        bool isFoundVertRect = false;
        bool isFoundHorizRect = false;
        for (int i = 0; i < greenContours.size(); i++)
        {
            tempRect = cv::minAreaRect(cv::Mat(greenContours[i]));
            area = tempRect.boundingRect().width * tempRect.boundingRect().height;
            if ((area > 100.0) && (area < 400))
            {
                // Test to see if width and height look like vertical (static) target
                // Note "width" is vertical due to orientation of camera
                if (tempRect.boundingRect().width > (tempRect.boundingRect().height * 2.5))
                {
                    isFoundVertRect = true;
                    vertRect = tempRect;
                }

                // Test to see if width and height look like horizontal (dynamic) target
                if (tempRect.boundingRect().height > (tempRect.boundingRect().width * 2.5))
                {
                    isFoundHorizRect = true;
                    horizRect = tempRect;
                }
            }
        }

        if ((!isFoundHorizRect) || (!isFoundVertRect))
        {
            isRightGreenTargetLit = false;
            isLeftGreenTargetLit = false;
        }
        else
        {
            // The dynamic target is to the left of both static targets or to the right of both static targets, so only need to test one of them
            // Note that due to camera on its side "X" is Near/Far and "Y" is Left/Right
            if (horizRect.center.y < vertRect.center.y)
            {
                isLeftGreenTargetLit = true;
            }
            else
            {
                isRightGreenTargetLit = true;
            }
        }

        CTestMonitor::getTicks(&timeNow);
        int timeLatencyThisCameraFrameMilliseconds = (int) CTestMonitor::getDeltaTimeMilliseconds(
                pFrame->m_timeAddedToQueue[(int) CVideoFrame::FRAME_QUEUE_WAIT_FOR_BLOB_DETECT],
                timeNow);

        pFrame->m_targetInfo.updateTargetInfo(
                timeSinceLastCameraFrameMilliseconds, timeLatencyThisCameraFrameMilliseconds, isRightGreenTargetLit, isLeftGreenTargetLit,
                isBlueFound, angleToBlueBallDegrees, distanceToBlueBallFeet,
                isRedFound, angleToRedBallDegrees, distanceToRedBallFeet);

        pFrame->updateAnnotationInfo(
                bestBallCircleBlue,
                bestBallCircleRed);

        m_gpioLed.setGreenLED((isBlueFound || isRedFound), pFrame->m_timeRemovedFromQueue[(int) CVideoFrame::FRAME_QUEUE_WAIT_FOR_BLOB_DETECT]);
    }
    catch (...)
    {
    }
}

bool CBallDetector::filterContoursToFindLargestBlob(
        const std::vector<std::vector<cv::Point> >& listContours,
        CBallCircle& bestBallCircle,
        float& angleToBallDegrees,
        float& distanceToBallFeet)
{
    bool isBallFound = false;
    bestBallCircle.init();
    angleToBallDegrees = -999.0;
    distanceToBallFeet = -1.0;

    unsigned int i = 0;
    CBallCircle tempBallCircle;
    std::vector<CBallCircle> listPossibleBallCircle;
    std::vector<cv::Point> contours_poly;
    for (i = 0; i < listContours.size(); i++)
    {
        approxPolyDP(cv::Mat(listContours[i]), contours_poly, 3, true);
        minEnclosingCircle((cv::Mat) contours_poly, tempBallCircle.m_ptCenter, tempBallCircle.m_radius);
        if (bestBallCircle.m_radius < tempBallCircle.m_radius)
        {
            isBallFound = true;
            bestBallCircle = tempBallCircle;
        }
    }
    if (isBallFound)
    {
        angleToBallDegrees = m_lookupTable[(int) bestBallCircle.m_ptCenter.x][(int) bestBallCircle.m_ptCenter.y].angleToBallDegrees;
        distanceToBallFeet = m_lookupTable[(int) bestBallCircle.m_ptCenter.x][(int) bestBallCircle.m_ptCenter.y].distanceToBallFeet;
    }
    return isBallFound;
}

bool CBallDetector::filterContoursToFindBallBySize(
        const std::vector<std::vector<cv::Point> >& listContours,
        CBallCircle& bestBallCircle,
        float& angleToBallDegrees,
        float& distanceToBallFeet)
{
    bool isBallFound = false;
    bestBallCircle.init();
    angleToBallDegrees = -999.0;
    distanceToBallFeet = -1.0;

    unsigned int i = 0;
    CBallCircle tempBallCircle;
    std::vector<CBallCircle> listPossibleBallCircle;
    std::vector<cv::Point> contours_poly;
    for (i = 0; i < listContours.size(); i++)
    {
        approxPolyDP(cv::Mat(listContours[i]), contours_poly, 3, true);
        minEnclosingCircle((cv::Mat) contours_poly, tempBallCircle.m_ptCenter, tempBallCircle.m_radius);
        if (!isTooSmallToBeABall(tempBallCircle))
        {
            listPossibleBallCircle.push_back(tempBallCircle);
        }
    }
    if (listPossibleBallCircle.size() == 1)
    {
        // Perfect - Found exactly one candidate 
        isBallFound = true;
        bestBallCircle = listPossibleBallCircle.at(0);
    }
    else if (listPossibleBallCircle.size() > 1)
    {
        // Take first candidate that is about the right size
        for (i = 0; i < listPossibleBallCircle.size(); i++)
        {
            if (!isTooBigToBeABall(listPossibleBallCircle.at(i)))
            {
                isBallFound = true;
                bestBallCircle = listPossibleBallCircle.at(i);
            }
        }
    }
    if (isBallFound)
    {
        angleToBallDegrees = m_lookupTable[(int) bestBallCircle.m_ptCenter.x][(int) bestBallCircle.m_ptCenter.y].angleToBallDegrees;
        distanceToBallFeet = m_lookupTable[(int) bestBallCircle.m_ptCenter.x][(int) bestBallCircle.m_ptCenter.y].distanceToBallFeet;
    }
    return isBallFound;
}

bool CBallDetector::isTooSmallToBeABall(const CBallCircle & ballCircle) const
{
    unsigned int pixelX = (unsigned int) ballCircle.m_ptCenter.x;
    unsigned int pixelY = (unsigned int) ballCircle.m_ptCenter.y;
    float pixelRadius = m_lookupTable[pixelX][pixelY].pixelRadius;

    return (ballCircle.m_radius < ((1.0 - m_tolerancePercentForRadius) * pixelRadius));
}

bool CBallDetector::isTooBigToBeABall(const CBallCircle & ballCircle) const
{
    unsigned int pixelX = (unsigned int) ballCircle.m_ptCenter.x;
    unsigned int pixelY = (unsigned int) ballCircle.m_ptCenter.y;
    float pixelRadius = m_lookupTable[pixelX][pixelY].pixelRadius;
    return (ballCircle.m_radius > ((1.0 + m_tolerancePercentForRadius) * pixelRadius));
}
